package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoystick;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 * This class contains all of the definitions and methods for controlling
 * the robot's drive train (chassis)
 * @author anne
 */
public class Chassis extends Subsystem {
    
    RobotDrive drive; // Instance of the class used to drive the robot's motors
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        /**
         * Set the default command for the drive train. The default
         * command is the command (if any) that will execute when there is
         * no other command running on the subsystem
         */
        setDefaultCommand(new DriveWithJoystick());
    }
    
    public Chassis() {
        // Note: modified from (2,1) in example for 2 motor robot
        
        /**
         * Initialize a drive instance to control driving the robot. Specify
         * a port number for each motor.  The 2010 robot has 4 motors-
         * 2 in the front and 2 in the back. 
         */
        drive= new RobotDrive(RobotMap.DRIVE_TRAIN_FRONT_LEFT_MOTOR,
                              RobotMap.DRIVE_TRAIN_REAR_LEFT_MOTOR,
                              RobotMap.DRIVE_TRAIN_FRONT_RIGHT_MOTOR,
                              RobotMap.DRIVE_TRAIN_REAR_RIGHT_MOTOR);
        drive.setSafetyEnabled(false);
    }
    
    public void straight() {
        /**
         * Perform arcade drive using 1.0 for the forward/backward motion (
         * move forward) and 0.0 for the left/right turn value (no turn).  
         * This should cause the robot to move forward
         */
        drive.arcadeDrive(1.0,0.0);
    }
    
    /**
     * This method causes the robot to turn left by performing an arcade drive
     */
    public void turnLeft() {
        /**
         * Perform arcade drive using 0 for the forward/backward motion (don't
         * move forward/backward) and 1.0 for the left/right turn value.  
         * This should cause it to turn left
         */
        drive.arcadeDrive(0.0,1.0); 
    }
    
    /**
     * This method performs joystick controlled driving using the
     * arcade drive
     * @param stick The joystick being used to drive the robot
     */
    public void driveWithJoystick(Joystick stick) {
        
        /**
         * Perform arcade drive using passed in joystick for control
         */
        drive.arcadeDrive(stick);
    }
}

